#!/usr/bin/env python 
# -*- coding: utf-8 -*-
 
import roslib;
import rospy  
import actionlib  
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
from scheduler_client import scheduler_client
import time
inital_pose = Pose(Point(4.9, 4.5, 0.000), Quaternion(0.000, 0.000, 1.000, -0.025))  
inital_flag = True
# 节点初始化 
rospy.init_node('move_test1', anonymous=True)    
# 订阅move_base服务器的消息  
move_base = actionlib.SimpleActionClient("/tb3_1/move_base", MoveBaseAction) 
rospy.loginfo("Waiting for move_base action server...")  
target = None
# 等待连接服务器，5s等待时间限制 
while move_base.wait_for_server(rospy.Duration(5.0)) == 0:
    rospy.loginfo("Connected to move base server")  

while(True):
    if inital_flag:
        curr_pose = inital_pose
        inital_flag = False

    else:
        curr_pose = target
    request = [curr_pose.position.x,curr_pose.position.y,curr_pose.position.z,curr_pose.orientation.x,curr_pose.orientation.y,curr_pose.orientation.z,curr_pose.orientation.w]
    response = scheduler_client(request)
    # 设定目标点  
    target = response 
    if response is None:
        break
    goal = MoveBaseGoal()  
    goal.target_pose.pose = target  
    goal.target_pose.header.frame_id = 'map'  
    goal.target_pose.header.stamp = rospy.Time.now()  

    rospy.loginfo("Going to: " + str(target))  

    # 向目标进发  
    move_base.send_goal(goal)  

    # 五分钟时间限制  
    finished_within_time = move_base.wait_for_result(rospy.Duration(120))   

    # 查看是否成功到达  
    if not finished_within_time:  
        move_base.cancel_goal()  
        rospy.loginfo("Timed out achieving goal")  
    else:  
        state = move_base.get_state()  
        if state == GoalStatus.SUCCEEDED:  
            rospy.loginfo("Goal succeeded!")
            time.sleep(3)
        else:  
            rospy.loginfo("Goal failed！ ")  

